#include "core/state_block/local_parametrization_quaternion.h"
#include "core/math/rotations.h"
#include <iostream>